import numpy as np
import logging
from collections import defaultdict
from utils import distance, solve_position

INF = 0x3F3F3F3F


class DV_Hop_Positioning:
    """
    使用范围：
        锚节点位置已知, 未知节点位置未知, 但是节点之间的
        邻接矩阵已知, 但不知道具体的距离
    (1)计算平均每跳距离
        首先初始化每个锚节点之间的距离,然后利用最短路径算法,求出两节点之间的最短跳数。
        每个锚节点根据记录的与其他锚节点的位置信息和相距跳数, 利用上式估算平均每跳的实际距离：
        其中(x_i, y_i)、(x_j, y_j)是信标节点i、j的坐标, h_j是信标节点i与j(i≠j)之间的跳数。
    (2)估算未知节点与已知节点的距离
        未知节点使用与它最近跳数的平均每跳距离，估算与所有锚节点的距离。
    (3)计算节点的位置
        根据前两个阶段我们得到未知节点节点到锚节点的估算距离之后，
        使用三边测量法计算出未知节点的位置坐标信息，实现对未知节点的定位。
    """

    def __init__(self, points_data, graph) -> None:
        archor_points = points_data.anchor_data_dict()
        unkonwn_points = points_data.unknown_data_dict()
        self.archor_points: dict = archor_points
        self.unknown_points: set = unkonwn_points.keys()
        self.hop_graph = defaultdict(lambda: defaultdict(lambda: INF))
        self.dis_graph = defaultdict(lambda: defaultdict(lambda: INF))
        self.all_average_dis = defaultdict(lambda: INF)

        def generate_hop_graph():
            logging.debug("开始计算最短跳数图")
            hop_graph = defaultdict(lambda: defaultdict(lambda: INF))
            points = set(points_data.get_all_points_name())
            for u in points:
                points.add(u)
                for v in graph.get_neighbors(u):
                    hop_graph[u][v] = 1
                    points.add(v)
            points_order = sorted(list(points))
            for i in points_order:
                hop_graph[i][i] = 0
            for k in points_order:
                for i in points_order:
                    if i == k or hop_graph[i][k] == INF:
                        continue
                    for j in points_order:
                        if j == k or j == i or hop_graph[k][j] == INF:
                            continue
                        hop_graph[i][j] = min(
                            hop_graph[i][j], hop_graph[i][k] + hop_graph[k][j]
                        )
            logging.debug("计算最短跳数图完成")
            # logging.debug(f"hop_graph: {hop_graph}")
            return hop_graph

        self.hop_graph = generate_hop_graph()

        def init_dis_graph():
            logging.debug("开始初始化距离图")
            dis_graph = defaultdict(lambda: defaultdict(lambda: INF))
            for u in archor_points:
                for v in archor_points:
                    if u == v:
                        dis_graph[u][v] = 0
                    else:
                        dis_graph[u][v] = distance(archor_points[u], archor_points[v])
            logging.debug("初始化距离图完成")
            return dis_graph

        self.dis_graph = init_dis_graph()

        def generate_all_average_dis():
            logging.debug("开始计算锚节点平均每跳距离")
            all_average_dis = defaultdict(lambda: INF)
            for i in archor_points:
                sum_dis, sum_hops = 0, 0
                for j in archor_points:
                    if i == j:
                        continue
                    sum_dis += distance(archor_points[i], archor_points[j])
                    sum_hops += self.hop_graph[i][j]
                all_average_dis[i] = sum_dis / sum_hops
            logging.debug(f"average_dis: {all_average_dis}")
            logging.debug("计算锚节点平均每跳距离完成")
            return all_average_dis

        self.all_average_dis = generate_all_average_dis()

    def predict_archor_points_dis(self):
        """
        估算已知节点到未知节点的距离
        已知节点使用与它的平均每跳距离，估算与所有未知节点的距离。
        结果 存储在 self.dis_graph 中
        """
        logging.debug("开始估算已知节点到未知节点的距离")
        hop_graph = self.hop_graph
        dis_graph = self.dis_graph
        for u in self.archor_points:
            predicted_single_hop_dis = self.all_average_dis[u]
            for v in self.unknown_points:
                dis_graph[u][v] = predicted_single_hop_dis * hop_graph[u][v]
                # logging.debug(f"已知节点{u}到未知节点{v}的距离为{dis_graph[u][v]}")
        logging.debug("结束估算已知节点到未知节点的距离")

    def predict_unknown_points_dis(self):
        """
        估算未知节点与已知节点的距离
        未知节点使用与它最近跳数的平均每跳距离，估算与所有锚节点的距离。
        补充：我的算法中
            如果有 n 个锚节点与未知节点的跳数相同，那么取这 n 个锚节点的平均距离。
        结果 存储在 self.dis_graph 中
        """
        logging.debug("开始估算未知节点与已知节点的距离")
        hop_graph = self.hop_graph
        dis_graph = self.dis_graph
        for u in self.unknown_points:
            # 获取 u 与锚节点的最短跳数 和 相应的锚节点
            predicted_hop = INF
            real_archors = set()
            for archor in self.archor_points:
                if hop_graph[u][archor] < predicted_hop:
                    real_archors.clear()
                    real_archors.add(archor)
                    predicted_hop = hop_graph[u][archor]
                elif hop_graph[u][archor] == predicted_hop:
                    real_archors.add(archor)
            if predicted_hop == INF:
                logging.warning(f"未知节点{u}无法定位")
                continue
            # 获取 u 估计的每跳距离
            # 使用 n 个锚节点的平均距离
            predicted_single_hop_dis = np.mean(
                np.array([self.all_average_dis[archor] for archor in real_archors])
            )
            logging.debug(f"未知节点{u}预计每跳距离{predicted_single_hop_dis}")
            for archor in self.archor_points:
                dis_graph[u][archor] = predicted_single_hop_dis * hop_graph[u][archor]
                # logging.debug(f"未知节点{u}到已知节点{archor}的距离为{dis_graph[u][archor]}")
        logging.debug("结束估算未知节点与已知节点的距离")

    def run(self):
        logging.debug("开始节点定位")
        self.predict_archor_points_dis()
        self.predict_unknown_points_dis()
        cal_points = dict()
        archor_points = self.archor_points
        archor_points_order = sorted(list(archor_points.keys()))
        for u in self.unknown_points:
            distances = [self.dis_graph[u][v] for v in archor_points_order]
            points = [archor_points[v] for v in archor_points_order]
            ux, uy = solve_position(distances, points)
            logging.debug(f"节点{u}的位置为({ux},{uy})")
            cal_points[u] = np.array([ux, uy])
        logging.debug("结束节点定位")
        return cal_points
